#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

// #include <math.h>

bool move_status = true;

void Init_State();
void Control();

moveit_msgs::RobotTrajectory Cartesian_Move(geometry_msgs::Pose start_pose,geometry_msgs::Pose target_pose,int interval_num);


int main(int argc, char** argv)
{
    ros::init(argc, argv, "hyxxi_tutorial");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();
    static const std::string PLANNING_GROUP = "gluon";
    //路径规划
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    //障碍规划
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    const moveit::core::JointModelGroup* joint_model_group =
        move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    while(ros::ok()) {

      std::vector<double> joint_home_positions(6, 0.0);
      move_group.setJointValueTarget(joint_home_positions);
      ROS_INFO("Go to home");
      move_group.move();
      ros::Duration(1).sleep();

      geometry_msgs::Pose target_pose;
      target_pose.orientation.w = 1.0;
      target_pose.position.x = 0.18;
      target_pose.position.y = -0.12;
      target_pose.position.z = 0.25;
      move_group.setPoseTarget(target_pose);
      ROS_INFO("Go to target_pose");
      move_group.move(); 

      moveit::planning_interface::MoveGroupInterface::Plan my_plan;
      bool success = false;

      geometry_msgs::Pose start_pose2;
      start_pose2.orientation.y = 0.70711;
      start_pose2.orientation.w = 0.70711;
      start_pose2.position.x = 0.30;
      start_pose2.position.y = 0.0;
      start_pose2.position.z = 0.10;
      move_group.setPoseTarget(start_pose2);
      ROS_INFO("Move to start_pose");
      move_group.move();

      // 笛卡尔多点目标点规划
      if(move_status)
      {
        move_status = false;
        // Cartesian_Move(target_pose1,start_pose2,3);

        //产生路点
        std::vector<geometry_msgs::Pose> waypoints;
        int interval_num = 3;
        float interval_x,interval_y,interval_z;
        float inte_siyuan_x,inte_siyuan_y,inte_siyuan_z,inte_siyuan_w;
        interval_x = (target_pose.position.x - start_pose2.position.x)/interval_num;
        interval_y = (target_pose.position.y - start_pose2.position.y)/interval_num;
        interval_z = (target_pose.position.z - start_pose2.position.z)/interval_num;
        inte_siyuan_x = (target_pose.orientation.x- start_pose2.orientation.x)/interval_num;
        inte_siyuan_y = (target_pose.orientation.y- start_pose2.orientation.y)/interval_num;
        inte_siyuan_z = (target_pose.orientation.z- start_pose2.orientation.z)/interval_num;
        inte_siyuan_w = (target_pose.orientation.w- start_pose2.orientation.w)/interval_num;

        geometry_msgs::Pose temp_pose = start_pose2;
        for(int i=0;i<interval_num;i++)
        {
          temp_pose.position.x += interval_x;
          temp_pose.position.y += interval_y;
          temp_pose.position.z += interval_z;
          temp_pose.orientation.w += inte_siyuan_w;
          waypoints.push_back(temp_pose);
          ROS_INFO("temo_pose %d: x-> %f y-> %f z-> %f",i,temp_pose.position.x,temp_pose.position.y,temp_pose.position.z);
        }

        // 笛卡尔空间下的路径规划
        moveit_msgs::RobotTrajectory trajectory;
        const double jump_threshold = 0.0;
        const double eef_step = 0.01;
        double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
        ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

        //执行运动
        move_group.execute(trajectory);
        ros::Duration(2).sleep();

        move_status = true;
      }

      // END_TUTORIAL

      ros::Duration(10).sleep();
    }
    ros::shutdown();
    return 0;
    }

// moveit_msgs::RobotTrajectory Cartesian_Move(geometry_msgs::Pose start_pose,geometry_msgs::Pose target_pose,int interval_num)
// {
//   // @ start_pose:起始位置
//   // @ target_pose:开始位置
//   float interval_x,interval_y,interval_z;
//   std::vector<geometry_msgs::Pose> waypoints;

//   move_group.setPoseTarget(start_pose);
//   ROS_INFO("Move to start_pose");
//   move_group.move();
//   ROS_INFO("-end start_pose-------------------------");

//   interval_x = (target_pose.position.x - start_pose.position.x)/interval_num;
//   interval_y = (target_pose.position.y - start_pose.position.y)/interval_num;
//   interval_z = (target_pose.position.z - start_pose.position.z)/interval_num;
  
//   geometry_msgs::Pose temp_pose = start_pose;
//   for(int i=0;i<=interval_num;i++)
//   {
//     temp_pose.position.x += interval_x*i;
//     temp_pose.position.y += interval_y*i;
//     temp_pose.position.z += interval_z*i;
//     waypoints.push_back(temp_pose);
//   }

//   moveit_msgs::RobotTrajectory trajectory;
//   const double jump_threshold = 0.0;
//   const double eef_step = 0.01;
//   double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
//   ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

//   // You can execute a trajectory like this.
//   int nrOfPoints = trajectory.joint_trajectory.points.size();
//   ROS_INFO("nrOfPoints %d", nrOfPoints);
//   for (int i = 0; i < nrOfPoints; i++) {
//     //ActuatorController::UnifiedID actuator = uIDArray.at(i);
//     int nrOfJoints = trajectory.joint_trajectory.points[i].positions.size();
//     for (int j = 0; j < nrOfJoints; j++) {
//       ROS_INFO("point[%d]: %s, %f", i, trajectory.joint_trajectory.joint_names[j].c_str(), 
//           trajectory.joint_trajectory.points[i].positions[j]);
//     }
//     ROS_INFO("-------------------------------------------");
//   }
//    move_group.execute(trajectory);
//    ros::Duration(2).sleep();

// }
// void Init_State()
// {
//    //Go to home
//   std::vector<double> joint_home_positions(6, 0.0);
//   move_group.setJointValueTarget(joint_home_positions);
//   ROS_INFO("Go to home");
//   move_group.move();
// }
// void Control()
// {
//   //运动控制


// }